import queue
import random
import math
import gc
import threading

import cv2
from bokeh.colors.groups import white
from datashape.discovery import edges
from memory_profiler import profile


class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None

    def __eq__(self, other):
        if not isinstance(other, Node):
            return NotImplemented
        return self.x == other.x and self.y == other.y

    def __hash__(self):
        return hash((self.x, self.y))

# 生成随机点
def get_random_node(width, height):
    return Node(random.randint(0, width), random.randint(0, height))


# 求距离
def distance(node1, node2):
    return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)

# 找到距离最近的节点
def get_nearest_node(node_list, random_node):
    return min(node_list, key=lambda node: distance(node, random_node))
# @profile(precision=4)
# profile(precision=4, stream=open('memory_profiler.log', 'w+'))
def multi_rrt_frontier_detect(image_path, start, image, height, width, color_image, node_list_global, frontier_list_global, thread_index=1, passwall=False, max_iter=100):
    gc.disable()
    # # 加载图片
    # image = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
    # height, width = image.shape
    # # 生成图
    # color_image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)

    # 开始节点
    start_node = Node(start[0], start[1])

    # node列表
    node_list = node_list_global
    node_list.append(start_node)
    # 边列表
    # edges_list = []
    # 边界列表
    frontier_list = frontier_list_global
    cv2.circle(color_image, (start_node.x, start_node.y), 1, (0, 0, 255), -1)  # 半径为1，颜色为绿色，-1表示填充
    sum_repeat = 0

    # 方向向量
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]

    # 定义灰色和白色的阈值
    # gray_value = 128
    gray_value_low = 100
    gray_value_top = 240
    # white_value = 255
    white_value_low = 249
    white_value_top = 255

    #迭代总次数
    num = 0

    # for _ in range(1000):
    #     edges_list.append([3, 4])
    for _ in range(max_iter):
        # 统计总次数
        num = num+1
        # 生成随机点
        random_node = get_random_node(width, height)
        # 获取最近的节点
        nearest_node = get_nearest_node(node_list, random_node)

        # 生成new节点
        theta = math.atan2(random_node.y - nearest_node.y, random_node.x - nearest_node.x)
        new_node = Node(
            int(nearest_node.x + 10 * math.cos(theta)),
            int(nearest_node.y + 10 * math.sin(theta))
        )
        # 防止溢出
        new_node.x = min(new_node.x, width-1)
        new_node.x = max(new_node.x, 0)
        new_node.y = min(new_node.y, height-1)
        new_node.y = max(new_node.y, 0)
        # new_node.parent = nearest_node
        # edges_list.append([3,4])
        # 画点
        cv2.circle(color_image, (new_node.x, new_node.y), 1, (0, 0, 0), -1)  # 半径为1，颜色为绿色，-1表示填充
        # 画线
        cv2.line(color_image, (nearest_node.x, nearest_node.y), (new_node.x, new_node.y),
                 switch_color(thread_index), 1)

        if passwall:
            node_list.append(new_node)
        elif image[new_node.y, new_node.x] >= 249:  # 不能是障碍物
            node_list.append(new_node)

        # 检查是否是边界
        if white_value_low <= image[new_node.y, new_node.x] <= white_value_top and any(gray_value_low <= image[new_node.y + ddy, new_node.x + ddx] <= gray_value_top for ddx, ddy in directions if
               0 <= new_node.x + ddx < width and 0 <= new_node.y + ddy < height):
            # xxxx = True
            # for point in frontier_list:
            #     if point.x == new_node.x and point.y == new_node.y:
            #         xxxx = False
            # if not xxxx:
            #     sum_repeat += 1
            frontier_list.append(new_node)
            # 查找到相邻的所有边界
            # find_connected_frontier(image, new_node, color_image, frontier_list, directions,
            #                         gray_value_low, gray_value_top, width, height, white_value_low,
            #                         white_value_top)
            # continue
            cv2.circle(color_image, (new_node.x, new_node.y), 3, (0, 0, 255), -1)  # 半径为1，颜色为绿色，-1表示填充

        # if distance(new_node, goal_node) < 10:
        #     goal_node.parent = new_node
        #     node_list.append(goal_node)
        #     print("找到了，通过"+str(num)+"次")
        #     break

    # 画路线
    # current_node = goal_node
    # while current_node.parent is not None:
    #     cv2.line(color_image, (current_node.x, current_node.y), (current_node.parent.x, current_node.parent.y),
    #              (0, 0, 0), 2)
    #     current_node = current_node.parent
    # print("探测到的边界总数:")
    # print(len(frontier_list))
    # print([(node.x, node.y) for node in frontier_list])  # 输出边界点的坐标
    # cv2.imshow('RRT frontier', color_image)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()
    # print("重复个数",sum_repeat)
    if thread_index == 0:
        global frontier_0
        frontier_0 = len(frontier_list)
    elif thread_index == 1:
        global frontier_1
        frontier_1 = len(frontier_list)
    elif thread_index == 2:
        global frontier_2
        frontier_2 = len(frontier_list)
    elif thread_index == 3:
        global frontier_3
        frontier_3 = len(frontier_list)
    return len(frontier_list)
def find_connected_frontier(image, start_node, color_image, frontier_list, directions, gray_value_low, gray_value_top,
                            width, height, white_value_low, white_value_top):
    # 八邻域
    directions_8 = [(-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (1, 1), (1, -1), (-1, 1)]
    # 创建一个队列，用于广度优先搜索
    q = queue.Queue()
    q.put(start_node)

    # 标记该节点为已访问
    visited = set()
    visited.add((start_node.x, start_node.y))

    # 开始广度优先搜索
    while not q.empty():
        node = q.get()

        # 将当前节点添加到frontier_list并在图片上标记
        # frontier_list.append(copy.deepcopy(node))  # 需要深拷贝节点，避免后续修改影响
        cv2.circle(color_image, (node.x, node.y), 3, (0, 0, 255), -1)

        # 遍历该节点的所有相邻节点
        for dy in range(-1, 2):  # 检查上下左右以及对角线邻域 (-1, 0, 1)
            for dx in range(-1, 2):
                if dx == 0 and dy == 0:
                    continue  # 跳过自身
                check_x = max(min(node.x + dx, width - 1), 0)
                check_y = max(min(node.y + dy, height - 1), 0)
                if 0 <= check_x < width and 0 <= check_y < height:
                    # 如果该点符合边界条件，并且没有访问过
                    if ((white_value_low <= image[check_y, check_x] <= white_value_top)
                            and (check_x, check_y) not in visited
                            and any(
                                gray_value_low <= image[check_y + ddy, check_x + ddx] <= gray_value_top for ddx, ddy
                                in
                                directions if 0 <= check_x + ddx < width and 0 <= check_y + ddy < height)):
                        new_node = Node(check_x, check_y)
                        new_node.parent = node  # 设置父节点，便于路径回溯

                        # 将该点标记为已访问，并加入队列
                        visited.add((new_node.x, new_node.y))
                        q.put(new_node)

                        # xxxx = True
                        # for point in frontier_list:
                        #     if point.x == new_node.x and point.y == new_node.y:
                        #         xxxx = False
                        # if not xxxx:
                        #     sum_repeat += 1

                        frontier_list.append(new_node)
                        # print("确实存在连续")
    return frontier_list
def switch_color(value):
    switch_dict = {
        0: (255, 0, 0),  # 蓝色
        1: (255, 191, 0), # 淡蓝色
        2: (0, 255, 0),  # 绿色
        3: (0, 165, 255), # 橙色
        4: (255, 0, 255), # 紫色
        5: (0, 0, 255)  # 红色
        # 6: (203, 192, 255), # 粉色
    }
    return switch_dict.get(value, (0, 255, 0))  # 提供默认值

# 全局数据
frontier_0 = 0
frontier_1 = 0
frontier_2 = 0
frontier_3 = 0
frontier_list_0 = []
frontier_list_1 = []
frontier_list_2 = []
frontier_list_3 = []
node_list_0 = []
node_list_1 = []
node_list_2 = []
node_list_3 = []
# 整体调度
def multi_rrt(img_path, start, passwall=False, max_iter=4000):
    image= cv2.imread(img_path, cv2.IMREAD_GRAYSCALE)
    # 加载图片
    height, width = image.shape
    # 生成图
    color_image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
    global frontier_0, frontier_1, frontier_2, frontier_3
    global frontier_list_0, frontier_list_1, frontier_list_2, frontier_list_3
    global node_list_0, node_list_1, node_list_2, node_list_3
    # print(height)
    # print(width)
    # 创建线程
    # thread0 = threading.Thread(target=multi_greedy_rrt_frontier_detect, args=(img_path, (600, 520), image, height, width,color_image, node_list_0, frontier_list_0, 0, passwall, max_iter))
    # thread1 = threading.Thread(target=multi_greedy_rrt_frontier_detect, args=(img_path, (400, 520), image, height, width,color_image, node_list_1, frontier_list_1, 1, passwall, max_iter))
    # thread2 = threading.Thread(target=multi_greedy_rrt_frontier_detect, args=(img_path, (640, 150), image, height, width,color_image, node_list_2, frontier_list_2, 2, passwall, max_iter))
    # thread3 = threading.Thread(target=multi_greedy_rrt_frontier_detect, args=(img_path, (640, 300), image, height, width,color_image, node_list_3, frontier_list_3, 3, passwall, max_iter))

    # thread0 = threading.Thread(target=multi_rrt_frontier_detect, args=(img_path, (250, 95), image, height, width, color_image, node_list_0, frontier_list_0, 0, passwall, max_iter))
    # thread1 = threading.Thread(target=multi_rrt_frontier_detect, args=(img_path, (300, 150), image, height, width, color_image, node_list_1, frontier_list_1, 1, passwall, max_iter))
    # thread2 = threading.Thread(target=multi_rrt_frontier_detect, args=(img_path, (280, 250), image, height, width, color_image, node_list_2, frontier_list_2, 2, passwall, max_iter))
    # thread3 = threading.Thread(target=multi_rrt_frontier_detect, args=(img_path, (100, 95), image, height, width, color_image, node_list_3, frontier_list_3, 3, passwall, max_iter))

    # thread0 = threading.Thread(target=multi_greedy_rrt_frontier_detect, args=(img_path, (250, 95), image, height, width, color_image, node_list_0, frontier_list_0, 0, passwall, max_iter))
    # thread1 = threading.Thread(target=multi_greedy_rrt_frontier_detect, args=(img_path, (100, 95), image, height, width, color_image, node_list_1, frontier_list_1, 1, passwall, max_iter))
    # thread2 = threading.Thread(target=multi_greedy_rrt_frontier_detect, args=(img_path, (280, 250), image, height, width, color_image, node_list_2, frontier_list_2, 2, passwall, max_iter))
    # thread3 = threading.Thread(target=multi_greedy_rrt_frontier_detect, args=(img_path, (100, 250), image, height, width, color_image, node_list_3, frontier_list_3, 3, passwall, max_iter))

    # DM
    thread0 = threading.Thread(target=multi_rrt_frontier_detect, args=(img_path, (2000, 4000), image, height, width, color_image, node_list_0, frontier_list_0, 0, passwall, max_iter))
    thread1 = threading.Thread(target=multi_rrt_frontier_detect, args=(img_path, (500, 3000), image, height, width, color_image, node_list_1, frontier_list_1, 1, passwall, max_iter))
    thread2 = threading.Thread(target=multi_rrt_frontier_detect, args=(img_path, (1800, 2000), image, height, width, color_image, node_list_2, frontier_list_2, 2, passwall, max_iter))
    thread3 = threading.Thread(target=multi_rrt_frontier_detect, args=(img_path, (1200, 1000), image, height, width, color_image, node_list_3, frontier_list_3, 3, passwall, max_iter))

    # 启动线程
    thread0.start()
    # print("thread0开始")
    thread1.start()
    # print("thread1开始")
    thread2.start()
    # print("thread2开始")
    thread3.start()
    # print("thread3开始")

    thread0.join()
    # print("thread0结束")
    thread1.join()
    # print("thread1结束")
    thread2.join()
    # print("thread2结束")
    thread3.join()
    # print("thread3结束")
    # print(len(frontier_list_0))
    # print(len(frontier_list_1))
    # print(len(frontier_list_2))
    # print(len(frontier_list_3))

    # 总个数
    # size = multi_greedy_rrt_frontier_detect(img_path, (600, 520), True)  # intel
    # size = multi_greedy_rrt_frontier_detect(img_path, (600, 120), True)  # intel

    # cv2.imshow('greedy-RRT frontier', color_image)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()

    size = frontier_0 + frontier_1 + frontier_2 + frontier_3
    # print(frontier_0)
    # print(frontier_1)
    # print(frontier_2)
    # print(frontier_3)
    # 全局数据
    frontier_0 = 0
    frontier_1 = 0
    frontier_2 = 0
    frontier_3 = 0
    frontier_list_0 = []
    frontier_list_1 = []
    frontier_list_2 = []
    frontier_list_3 = []
    node_list_0 = []
    node_list_1 = []
    node_list_2 = []
    node_list_3 = []
    return size
if __name__ == '__main__':
    # 使用该函数
    rrt_frontier_detect('path_to_your_image.jpg', (600, 520))
